#include "Robot.h"


void Robot::TeleopInit()
{


	rightOn = false;
	rightPressed = false;

	leftOn = false;
	leftPressed = false;

	isInFirstGear = false;

	cam1on = true;
	camTogglePressed = false;

	presetMode = -1;

	shift.Set(true);
	shiftRetract.Set(false);
	isInFirstGear = true;

	comp.Start();

	teleopCount = 0;



	// acquire images
	IMAQdxStartAcquisition(session);
}

void Robot::TeleopPeriodic() {

	IMAQdxGrab(session, frame, true, NULL);
	if(imaqError != IMAQdxErrorSuccess) {
		DriverStation::ReportError("IMAQdxGrab error: " + std::to_string((long)imaqError) + "\n");
	} else {

		CameraServer::GetInstance()->SetImage(frame);
	}

	SmartDashboard::PutString("DB/String 9", std::to_string(liftEncoder.GetDistance()));



	myRobot.TankDrive((fabs(leftStick.GetY()) > JOYSTICKDEADSPACE ? leftStick.GetY() : 0.0), (fabs(rightStick.GetY()) > JOYSTICKDEADSPACE ? rightStick.GetY() : 0.0));




	if(xbox.GetRawButton(XBOX_START)) {
		if(!camTogglePressed) {
			cam1on = !cam1on;
			// stop image acquisition
			IMAQdxStopAcquisition(session);
			// create an image
			//frame = imaqCreateImage(IMAQ_IMAGE_RGB, 0);
			//the camera name (ex "cam0") can be found through the roborio web interface
			imaqError = IMAQdxOpenCamera(cam1on ? "cam1" : "cam0", IMAQdxCameraControlModeController, &session);
			if(imaqError != IMAQdxErrorSuccess) {
				DriverStation::ReportError("IMAQdxOpenCamera error: " + std::to_string((long)imaqError) + "\n");
			}
			imaqError = IMAQdxConfigureGrab(session);
			if(imaqError != IMAQdxErrorSuccess) {
				DriverStation::ReportError("IMAQdxConfigureGrab error: " + std::to_string((long)imaqError) + "\n");
			}

			SmartDashboard::PutString("DB/String 4", cam1on ?  "Top View" : "Mid View" );
			// acquire images
			IMAQdxStartAcquisition(session);

		}
		camTogglePressed = true;
	}
	else {
		camTogglePressed = false;
	}





	if(xbox.GetRawButton(XBOX_Y) && liftEncoder.GetDistance() < 46)
	{
		liftMotor.Set(1.0);
		presetMode = -1;
		if(teleopCount % 10 == 0)
		{
			SmartDashboard::PutString("DB/String 1", "Lift Manually Going Up");
		}

	}
	else if(xbox.GetRawButton(XBOX_A) && liftSensor.Get())
	{
		liftMotor.Set(-1.0);
		presetMode = -1;
		if(teleopCount % 10 == 0)
		{
			SmartDashboard::PutString("DB/String 1", "Lift Manually Going Down");
		}

	}
	else
	{
		liftMotor.Set(0.0);
	}

	if(!liftSensor.Get()){
		if(teleopCount % 10 == 0)
		{
			SmartDashboard::PutString("DB/String 1", "Lift Down");
			liftEncoder.Reset();
		}

	}

	//When the B button is pressed the right arm change position
	if(xbox.GetRawButton(XBOX_RBUMP)) {
		if(!rightPressed) {
			rightOn = !rightOn;
			rightArm.Set(rightOn);
			rightArmRetract.Set(!rightOn);
			if(teleopCount % 10 == 0)
			{
				SmartDashboard::PutString("DB/String 3", rightOn ? "Right Open" : "Right Closed");
			}
		}
		rightPressed = true;
	}
	else {
		rightPressed = false;
	}

	//When the X button is pressed the left change position
	if(xbox.GetRawButton(XBOX_LBUMP)) {
		if(!leftPressed) {
			leftOn = !leftOn;
			leftArm.Set(leftOn);
			leftArmRetract.Set(!leftOn);
			if(teleopCount % 10 == 0)
			{
				SmartDashboard::PutString("DB/String 2", leftOn ? "Left Open":"Left Closed");
			}
		}
		leftPressed = true;
	}
	else {
		leftPressed = false;
	}





	//MORE SHIFTING
	if (rightStick.GetRawButton(2)) {
		//FIRST GEAR
		if (!isInFirstGear) {
			if(teleopCount % 10 == 0)
			{
				SmartDashboard::PutString("DB/String 0", "In First Gear");
			}
			shift.Set(true);
			shiftRetract.Set(false);
			isInFirstGear = true;

		}
	}
	//MORE SHIFTING
	if (rightStick.GetRawButton(3)) {
		//SECOND GEAR
		if (isInFirstGear) {
			if(teleopCount % 10 == 0)
			{
			SmartDashboard::PutString("DB/String 0", "In Second Gear");
			}
			shift.Set(false);
			shiftRetract.Set(true);
			isInFirstGear = false;
		}
	}



	switch(presetMode) {
	case 0:
		if(!liftSensor.Get()) {
			liftMotor.Set(-1.0);
			presetMode = -1;
			liftEncoder.Reset();
			//SmartDashboard::PutString("DB/String 1", "Lift Going Down");
		}
		else {
			liftMotor.Set(0.0);
			presetMode = -1;

		}

		break;

	//Copy code to do more cases
	case 1:
		//SmartDashboard::PutString("DB/String 1", "Lift Mode 2");
		if(liftEncoder.GetDistance() > 2.5 ) {
			liftMotor.Set(-1.0);
		}
		else if(liftEncoder.GetDistance() < 3.5) {
			liftMotor.Set(1.0);
		}
		else {
			liftMotor.Set(0.0);
			presetMode = -1;
		}
		break;

	case 2:
		//SmartDashboard::PutString("DB/String 1", "Lift Mode 3");
		if(liftEncoder.GetDistance() >  3) {//+853?
			liftMotor.Set(-1.0);
		}
		else if(liftEncoder.GetDistance() < 4) {//+853?
			liftMotor.Set(1.0);
		}
		else {
			liftMotor.Set(0.0);
			presetMode = -1;
		}
		break;

	case 3:
		//SmartDashboard::PutString("DB/String 1", "Lift Mode 4");
		if(liftEncoder.GetDistance() > 7) {//+853?
			liftMotor.Set(-1.0);
		}
		else if(liftEncoder.GetDistance() < 9) {//+853?
			liftMotor.Set(1.0);
		}
		else {
			liftMotor.Set(0.0);
			presetMode = -1;
		}
		break;

	case 4:
	//	SmartDashboard::PutString("DB/String 1", "Lift Mode 5");
		if(liftEncoder.GetDistance() > 36.5 ) {//+853?
			liftMotor.Set(-1.0);
		}
		else if(liftEncoder.GetDistance() < 38.5   ) {//+853?
			liftMotor.Set(1.0);
		}
		else {
			liftMotor.Set(0.0);
			presetMode = -1;
		}
		break;

	case 5:
		//SmartDashboard::PutString("DB/String 1", "Lift Mode 6");
		if(liftEncoder.GetDistance() > 48.5 ) {//+853?
			liftMotor.Set(-1.0);
		}
		else if(liftEncoder.GetDistance() < 50.5) {//+853?
			liftMotor.Set(1.0);
		}
		else {
			liftMotor.Set(0.0);
			presetMode = -1;

		}
		break;

	case 6:
		//SmartDashboard::PutString("DB/String 1", "Lift Mode 7");
		if(liftEncoder.GetDistance() > 19) {//+853?
			liftMotor.Set(-1.0);
		}
		else if(liftEncoder.GetDistance() < 21) {//+853?
			liftMotor.Set(1.0);
		}
		else {
			liftMotor.Set(0.0);
			presetMode = -1;
		}
		break;

	case 7:
		//SmartDashboard::PutString("DB/String 1", "Lift Mode 8");
		if(liftEncoder.GetDistance() > 31) {//+853?
			liftMotor.Set(-1.0);
		}
		else if(liftEncoder.GetDistance() < 33) {//+853?
			liftMotor.Set(1.0);
		}
		else {
			liftMotor.Set(0.0);
			presetMode = -1;
		}
		break;

	case 8:
		//SmartDashboard::PutString("DB/String 1", "Lift Mode 9");
		if(liftEncoder.GetDistance() > 43) {//+853?
			liftMotor.Set(-1.0);
		}
		else if(liftEncoder.GetDistance() < 45) { //+853?
			liftMotor.Set(1.0);
		}
		else {
			liftMotor.Set(0.0);
			presetMode = -1;
		}
		break;
	}


	teleopCount ++;

}
